//
// Created by 86137 on 2024/1/25.
//

#include "motor_chassis_move.h"

extern rc_device_t g_rc;
extern float wheel_round[4];
extern float wheel_rpm[4];
extern float wheel_theta[4];
extern float wheel_cycle[4];
extern rc_blue_t blue_rc;
void motor_chassis_move(void *argument){
    static LK_motor lk_motor1(&hcan1, nullptr, 0, 1, ms_6015);
    static LK_motor lk_motor2(&hcan1, nullptr, 0, 2, ms_6015);
    static LK_motor lk_motor3(&hcan1, nullptr, 0, 3, ms_6015);
    static LK_motor lk_motor4(&hcan1, nullptr, 0, 4, ms_6015);
    lk_motor1.motor_control(3);
    lk_motor2.motor_control(3);
    lk_motor3.motor_control(3);
    lk_motor4.motor_control(3);
    lk_motor1.velPid.pid_reset(1.0f, 0.1f, 3, 0, 0);
    lk_motor1.posPid.pid_reset(1.0f, 0.1f, 5, 0, 0);
    lk_motor2.velPid.pid_reset(1.0f, 0.1f, 4, 0, 0);
    lk_motor2.posPid.pid_reset(1.0f, 0.1f, 4, 0, 0);
    lk_motor3.velPid.pid_reset(1.0f,0.1f,2.3f,0,0);
    lk_motor3.posPid.pid_reset(1.0f,0.1f,2.5f,0,0);
    lk_motor4.velPid.pid_reset(1.0f, 0.1f, 3.3f, 0, 0);
    lk_motor4.posPid.pid_reset(1.0f, 0.1f, 3, 0, 0);
    static DJI_motor dji_motor1(&hcan2, nullptr, 1, DJI_M3508, 1);
    static DJI_motor dji_motor2(&hcan2, nullptr, 2, DJI_M3508, 1);
    static DJI_motor dji_motor3(&hcan2, nullptr, 3, DJI_M3508, 0);
    static DJI_motor dji_motor4(&hcan2, nullptr, 4, DJI_M3508, 0);

//    DM_motor dm_motor_test(&hcan1,nullptr,0x000,0x001);
//    pwm_motor pwm_motor_test(&htim1,TIM_CHANNEL_2);

for(;;){
chassis_data_calculate();//计算舵轮与行进轮的相关速度
chassis_round_calculate();//
if(g_rc.is_lost){
    lk_motor1.motor_set_current(0);
    lk_motor2.motor_set_current(0);
    lk_motor3.motor_set_current(0);
    lk_motor4.motor_set_current(0);
    dji_motor1.motor_set_current(0);
    dji_motor2.motor_set_current(0);
    dji_motor3.motor_set_current(0);
    dji_motor4.motor_set_current(0);
}
if(g_rc.rc_info.sw.sw_l==RC_SW_UP || blue_rc.raw_blue_data!=0) {
    lk_motor1.motor_set_rounds(0.819f+wheel_round[0]);
    lk_motor2.motor_set_rounds(0.994f+wheel_round[1]);
    lk_motor3.motor_set_rounds(0.863f+wheel_round[2]);
    lk_motor4.motor_set_rounds(0.850f+wheel_round[3]);
    dji_motor1.motor_set_speed(wheel_rpm[0]);
    dji_motor2.motor_set_speed(wheel_rpm[1]);
    dji_motor3.motor_set_speed(wheel_rpm[2]);
    dji_motor4.motor_set_speed(wheel_rpm[3]);
}
else if(g_rc.rc_info.sw.sw_l==RC_SW_DOWN){
    lk_motor1.motor_set_rounds(0.82f+wheel_theta[0]);
    lk_motor2.motor_set_rounds(0.99f+wheel_theta[1]);
    lk_motor3.motor_set_rounds(0.87f+wheel_theta[2]);
    lk_motor4.motor_set_rounds(0.85f+wheel_theta[3]);
    dji_motor1.motor_set_rounds(wheel_cycle[0]);
    dji_motor2.motor_set_rounds(wheel_cycle[1]);
    dji_motor3.motor_set_rounds(wheel_cycle[2]);
    dji_motor4.motor_set_rounds(wheel_cycle[3]);
    if(dji_motor1.motor_get_rounds()==wheel_cycle[0]){
        chassis_round_update();
    }

}
osDelay(1);
}
}